#include <iostream>
#include <Eigen/Dense>
#include <sophus/se3.hpp>

int main() {
    // 定义欧拉角
    double yaw = M_PI / 4;  // Yaw (绕Z轴旋转)
    double pitch = M_PI / 6; // Pitch (绕Y轴旋转)
    double roll = M_PI / 8;  // Roll (绕X轴旋转)

    // 将旋转向量转化为旋转矩阵
    Sophus::SO3d SO3_R = Sophus::SO3d::exp(Sophus::Vector3d(yaw, pitch, roll));
    // 将旋转矩阵转化为四元数
    Eigen::Quaterniond q = SO3_R.unit_quaternion();

    // 输出四元数
    std::cout << "Quaternion: " << q.coeffs().transpose() << std::endl;

    return 0;
}
